/*
 * (C) Copyright 2005 Davide Brugali, Marco Torchiano
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA
 * 02111-1307  USA
 */

package moro;

/**
 * Title:        The MObile RObot Simulation Environment
 * Description:  The laser accepts commands from the controller and executes them in steps.
 *               The most important tasks are rotating the laser and getting measurements
 *               from the environment.
 * Copyright:    Copyright (c) 2001
 * Company:      Universit� di Bergamo
 * @author Davide Brugali
 * @version 1.0
 */

import java.awt.Color;
import java.awt.Graphics;
import java.awt.Polygon;
import java.awt.geom.Line2D;
import java.awt.geom.Point2D;
import java.util.ArrayList;

public class Laser extends Sensor{
	

	Measure detectMeasure = null;
	ArrayList<Measure> scanMeasures = new ArrayList<Measure>();

	public Laser(String name, Robot robot, Position localPos, Environment environment) {
		super(name, robot, localPos, environment);
		bgColor = Color.cyan;
		this.addPoint(0, 2);
		this.addPoint(100, 2);
		this.addPoint(100, -2);
		this.addPoint(0, -2);
	}

	public double read(boolean first) {
		Point2D centre = new Point2D.Double(localPos.getX(), localPos.getY());
		Point2D front = new Point2D.Double(localPos.getX() + range * Math.cos(localPos.getT()),
				localPos.getY() + range * Math.sin(localPos.getT()));
		// reads the robot's position
		robot.readPosition(robotPos);
		// center's coordinates according to the robot position
		robotPos.rototras(centre);
		// front's coordinates according to the robot position
		robotPos.rototras(front);

		double minDistance = -1.0;
		for(int i=0; i < environment.obstacles.size(); i++) {
			// This is really dirty: the laser uses direct access to environment's obstacles
			Obstacle obstacle = (Obstacle) environment.obstacles.get(i);
			if(obstacle.opaque)
				continue;
			double dist = pointToObstacle(obstacle.polygon, centre, front, first);
			if(minDistance == -1.0 || (dist > 0 && dist < minDistance)) {
				minDistance = dist;
				if(minDistance > -1 && first) {
					return minDistance;
				}
			}
		}
		if(minDistance > 0)
			return minDistance;
		return -1.0;
	}


}
